#include "speed_cntr.h"
#include "user_device.h"
#include "modbus.h"
#include "gpio.h"


//! Cointains data for timer interrupt.
SpeedRampData srd[MACHINE_COUNT] = {0};

SRD_DESC srd_desc[MACHINE_COUNT] =
{
    { TIMER2,  TIMER_CH_0, IO_S1_HOME,   IO_S1_LIMIT,   NULL_IN,  NULL_IN,   &srd[S1] },
    { TIMER7,  TIMER_CH_0, IO_S2_HOME,   IO_S2_LIMIT,   NULL_IN,  NULL_IN,   &srd[S2] },
    { TIMER3,  TIMER_CH_0, IO_S3_HOME,   IO_S3_LIMIT,   NULL_IN,  NULL_IN,   &srd[S3] },
    { TIMER13, TIMER_CH_0, IO_S4_HOME,   IO_S4_LIMIT,   NULL_IN,  NULL_IN,   &srd[S4] },
};

// 设置电机细分,并使能
void Set_Micro(void)
{
}

void InitMachine(void)
{
    srd[S1].role = S1;
    srd[S2].role = S2;
    srd[S3].role = S3;
    srd[S4].role = S4;

    srd[S1].reset_step = S1_RESET_BUFFER_STEP;
    srd[S2].reset_step = S2_RESET_BUFFER_STEP;
    srd[S3].reset_step = S3_RESET_BUFFER_STEP;
    srd[S4].reset_step = S4_RESET_BUFFER_STEP;

    srd[S1].limit_step = S1_LIMIT_BUFFER_STEP;
    srd[S2].limit_step = S2_LIMIT_BUFFER_STEP;
    srd[S3].limit_step = S3_LIMIT_BUFFER_STEP;
    srd[S4].limit_step = S4_LIMIT_BUFFER_STEP;

    srd[S1].stop_step = S1_STOP_BUFFER_STEP;
    srd[S2].stop_step = S2_STOP_BUFFER_STEP;
    srd[S3].stop_step = S3_STOP_BUFFER_STEP;
    srd[S4].stop_step = S4_STOP_BUFFER_STEP;

    srd[S1].v_lock_pos = V1_LOCK_STEP;
    srd[S2].v_lock_pos = V2_LOCK_STEP;
    srd[S3].v_lock_pos = V3_LOCK_STEP;
    srd[S4].v_lock_pos = V4_LOCK_STEP;

    srd[S1].v_release_pos = V1_LOCK_STEP;
    srd[S2].v_release_pos = V2_LOCK_STEP;
    srd[S3].v_release_pos = V3_LOCK_STEP;
    srd[S4].v_release_pos = V4_LOCK_STEP;

    srd[S1].limit_target = rw_word.mp[S1].max_distance;
    srd[S2].limit_target = rw_word.mp[S2].max_distance;
    srd[S3].limit_target = rw_word.mp[S3].max_distance;
    srd[S4].limit_target = rw_word.mp[S4].max_distance;

    srd[S1].reset_monitor_step = srd[S1].limit_step;
    srd[S2].reset_monitor_step = srd[S2].limit_step;
    srd[S3].reset_monitor_step = srd[S3].limit_step;
    srd[S4].reset_monitor_step = srd[S4].limit_step;

    srd[S1].micro = 0;
    srd[S2].micro = 0;
    srd[S2].micro = 0;
    srd[S4].micro = V_MISCRO;

    srd[S1].coder_self = 0;
    srd[S2].coder_self = 0;
    srd[S3].coder_self = 0;
    srd[S4].coder_self = 1;

    Set_Micro();

    IO_Set(IO_S1_LOCK);
    IO_Set(IO_S2_LOCK);
    IO_Set(IO_S3_LOCK);
    IO_Set(IO_S4_LOCK);

    IO_Reset(IO_S1_EN);
    IO_Reset(IO_S2_EN);
    IO_Reset(IO_S3_EN);
    IO_Reset(IO_S4_EN);
}

void speed_cntr_Move(SpeedRampData *srd, long step, unsigned int speed)
{
    if (step == 0)
        return;

    // Set direction from sign on step value.
    if(step < 0)
    {
        srd->dir = CCW;
        srd->step_offset = -1;
        step = -step;
    }
    else
    {
        srd->dir = CW;
        srd->step_offset = 1;
    }

    // 最大速度对应的速度表索引
    // n = speed^2 / (2*alpha*accel)
    srd->max_index = SPEED2INDEX(speed);
    if (srd->max_index > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->max_index = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    if (srd->first_stage_speed  > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->first_stage_speed  = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    if (srd->second_stage_speed > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->second_stage_speed = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;

    srd->speed_index = 0;
    srd->step_left = step;
    srd->run_state = MOTION_STATE_RUN;

    srd->running = 1;
    srd->need_cleanup = 0;
    srd->start_time = SystemTick();
    srd->step_count = 0;
    srd->init(srd->role);
}

void speed_update(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;

    if(srd->coder_self)
    {
        if( ++srd->micro_step < (srd->micro >> 1))
            return;
        srd->micro_step = 0;
    }

    Set_Timer(role);

    if (srd->run_state == MOTION_STATE_STOP)
    {
        Stop_Timer(role);
        srd->need_cleanup = 1;
    }
    else
    {
        // 更新剩余步数
        srd->step_left--;
        srd->step_count++;

        // 走一步, 调用更新函数, 用以检查传感器状态, 更新剩余步数等
        srd->update(role);
        if (srd_desc[role].limit && IO_Read(srd_desc[role].limit))
        {
            srd->dir ? srd->reset_monitor_step++ : srd->reset_monitor_step--;
        }

        if (srd->step_left <= 0)
        {
            srd->run_state = MOTION_STATE_STOP;
        }
        else
        {
          /*  // 电机在起始阶段和结束阶段的最大速度
            if (srd->step_count < srd->first_stage_step)
                max_index = srd->first_stage_speed;
            if (srd->step_count > srd->second_stage_step)
                max_index = srd->second_stage_speed;
            else
                max_index = srd->max_index*/;

            // 更新下一步速度
            if (srd->speed_index > srd->step_left)
                srd->speed_index -= srd->speed_index / (ushort)srd->step_left;
            if (srd->speed_index > srd->max_index)
                srd->speed_index--;
            else if (srd->speed_index < srd->max_index)
                srd->speed_index++;
        }
    }
}


